/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 ***************************************************************************/

#include "imudevice.h"
#include "domparser.h"
#include <QDebug>
#include <QSerialPortInfo>
#include "serialstructdefine.h"
#include "modbus.h"


#define IF_USE_IMU 1

ImuDevice::ImuDevice(QString configFileNameIn)
{

    isInitialOkStatus = false;
    requestCount=0;
    debugFlag=0;
    isUseImu=false;
    modbusMaster=NULL;
    if(IF_USE_IMU)
    {
        serialPortConfig = new SerialPortConfig;
        serialPort_device = new QSerialPort;
        // 初始化配置文件
        int tmpKey=initialConfigFile(configFileNameIn);
        if(1==tmpKey)
        {
            if(isUseImu)
            {
                // 初始化串口
                tmpKey=initalSerialPort();
    //            tmpKey=initialModbusMaster(serialPortConfig,0);
                if(1==tmpKey)
                {
                    serialPort_device->setReadBufferSize(50);
                    QObject::connect(serialPort_device, SIGNAL(readyRead() ), this, SLOT(receiveData()));
                    isInitialOkStatus = true;
                }
            }
            else
            {
                isInitialOkStatus = true;
            }

        }



    }
    else
    {
        isInitialOkStatus=true;
    }


}

int ImuDevice::getImuDeviceData(ImuDataInfo &dataOut)
{
    dataOut=imuData;
    return 1;
}

int ImuDevice::receiveImuDeviceData(ImuDataInfo &dataOut)
{
    if(isInitialOkStatus)
    {
        requestCount++;
//        receiveData();//不能在非信号槽线程调用吗？会崩溃．
        dataOut=imuData;
        if(requestCount>maxTimeOutCount)
        {
            qDebug()<<"error ,,,ImuDevice::getImuDeviceData 超时";
            return -1;
        }
        else
        {
            return 1;
        }
    }
    return 0;


}

bool ImuDevice::isUseThisModule()
{
    return isUseImu;
}

bool ImuDevice::isInitialOk()
{
    return isInitialOkStatus;
}


int ImuDevice::initialConfigFile(QString configFileName)
{
    qDebug() << "BatteryMonitor::initialBatteryByConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {

        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseImu", ok );
    if( !ok )
    {
            return -2;
    }
    domparser.readXml(domNode, isUseImu);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "imuVendorId", ok );
    if( !ok )
    {
            return -3;
    }
    int tmpVendor;
    domparser.readXml(domNode, tmpVendor);
    imuVendorId=(E_IMU_VENDOR_ID)tmpVendor;

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "deviceCodeId", ok );
    if( !ok )
    {
            return -3;
    }
    domparser.readXml(domNode, deviceCodeId);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "maxTimeOutCount", ok );
    if( !ok )
    {
            return -30;
    }
    domparser.readXml(domNode, maxTimeOutCount);


    //　读取串口配置参数结点
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "SerialSetting", ok );
    if(!ok)
    {

        return -4;
    }

    domparser.readXml( domparser.findSubNode(domNode, "busType", ok), \
                       serialPortConfig->busType );
    if( !ok  )
    {

        return -2;
    }

    // 读取串口设备文件
    domparser.readXml(domparser.findSubNode(domNode, "serialDeviceName", ok),\
                      serialPortConfig->serialDeviceName );
    if( !ok  )
    {

        return -3;
    }

    // 读取isUseUsb
    domparser.readXml(domparser.findSubNode(domNode, "isUseUsbDeviceSerialCode", ok),\
                      serialPortConfig->isUseUsbDeviceSerialCode );
    if( !ok  )
    {

        return -3;
    }


    // 读取配置文件波特率
    int baud;
    domparser.readXml(domparser.findSubNode(domNode, "baud" ,ok), baud);
    if( !ok )
    {

        return -4;
    }
    serialPortConfig->baudRate = (QSerialPort::BaudRate)baud;


    // 读取配置文件校验位
    QString parity;
    int parityNumber;
    domparser.readXml(domparser.findSubNode(domNode, "parity", ok), parity);
    if( !ok )
    {
        return -5;
    }

    if(parity == "NONE")
    {
        parityNumber = 0;
    }
    else if (parity == "EVEN")
    {
        parityNumber = 2;
    }
    else if (parity == "ODD")
    {
        parityNumber= 3;
    }
    else
    {

        return -6;
    }
    serialPortConfig->parity = (QSerialPort::Parity)parityNumber;


    //　读取配置文件数据位
    int dataBits;
    domparser.readXml(domparser.findSubNode(domNode, "data_bit", ok), dataBits);
    if( !ok )
    {

        return -7;
    }
    serialPortConfig->dataBits = (QSerialPort::DataBits)dataBits;


    // 读取配置文件停止位
    int stopBits;
    domparser.readXml(domparser.findSubNode(domNode, "stop_bit", ok), stopBits );
    if( !ok )
    {

        return -8;
    }
    serialPortConfig->stopBits = (QSerialPort::StopBits)stopBits;
    qDebug() << "BatteryMonitor::initialBatteryByConfigFile ==== 读取串口stop_bit:" \
             << serialPortConfig->stopBits;

    domparser.readXml(domparser.findSubNode(domNode, "updatePeriod", ok), serialPortConfig->updatePeriod);
    if( !ok )
    {

        return -9;
    }
    qDebug() << "BatteryMonitor::initialBatteryByConfigFile ==== 读取串口updatePeriod:" \
             << serialPortConfig->updatePeriod;



    domparser.closeXml();
    return 1;

}


void ImuDevice::decodeIMUData(unsigned char dataIn[], ImuDataInfo &dataOut)
{
    //没有校验数据
    double a[3],w[3],Angle[3],T;
    switch(dataIn[1])
    {
    case 0x51:
        dataOut.acc_x = (short(dataIn[3]<<8|dataIn[2]))/32768.0*16;
        dataOut.acc_y = (short(dataIn[5]<<8|dataIn[4]))/32768.0*16;
        dataOut.acc_z = (short(dataIn[7]<<8|dataIn[6]))/32768.0*16;
//        T = (short(dataIn[9]<<8|dataIn[8]))/340.0+36.25;
//        printf("a = %4.3f\t%4.3f\t%4.3f\t\r\n",a[0],a[1],a[2]);
        break;
    case 0x52:
        dataOut.rotateVel_x = (short(dataIn[3]<<8|dataIn[2]))/32768.0*2000;
        dataOut.rotateVel_y = (short(dataIn[5]<<8|dataIn[4]))/32768.0*2000;
        dataOut.rotateVel_z= (short(dataIn[7]<<8|dataIn[6]))/32768.0*2000;
//        T = (short(dataIn[9]<<8|dataIn[8]))/340.0+36.25;
//        printf("w = %4.3f\t%4.3f\t%4.3f\t\r\n",w[0],w[1],w[2]);
        break;
    case 0x53:
        dataOut.roll = (short(dataIn[3]<<8|dataIn[2]))/32768.0*180;
        dataOut.pitch = (short(dataIn[5]<<8|dataIn[4]))/32768.0*180;
        dataOut.yaw = (short(dataIn[7]<<8|dataIn[6]))/32768.0*180;
//        T = (short(dataIn[9]<<8|dataIn[8]))/340.0+36.25;
//        printf("Angle = %4.2f\t%4.2f\t%4.2f\tT=%4.2f\r\n",Angle[0],Angle[1],Angle[2],T);
        break;
    }
}

int ImuDevice::initalSerialPort()
{
    bool isFinded=false;
    /**
     * @brief serialPortInfo            串口信息对象
     */
    QSerialPortInfo serialPortInfo;
    foreach ( serialPortInfo, serialPortInfo.availablePorts() )
    {
        qDebug() << "Name : " << serialPortInfo.portName();
        qDebug() << "Description : " << serialPortInfo.description();
        qDebug() << "serialNumber: " << serialPortInfo.serialNumber();
        qDebug() << "productIdentifier:" << serialPortInfo.productIdentifier();
        if(1==serialPortConfig->isUseUsbDeviceSerialCode)
        {
            if( serialPortInfo.serialNumber() == serialPortConfig->serialDeviceName)
            {
                // 设置端口号
                serialPort_device->setPortName(serialPortInfo.portName());
                qDebug()<<"find COM"<<serialPortInfo.portName();
                isFinded=true;
                break;
            }
        }
        else
        {
            if( serialPortInfo.portName() == serialPortConfig->serialDeviceName )
            {
                // 设置端口号
                serialPort_device->setPortName(serialPortConfig->serialDeviceName);
                qDebug()<<"find COM"<<serialPortInfo.portName();
                isFinded=true;
                break;
            }
        }

    }
    if(false==isFinded)
    {
        qDebug() << "ImuDevice 未找到com设备";;
        return -1;
    }


    if( serialPort_device->open(QIODevice::ReadOnly) )
    {
       // 配置串口参数
       serialPort_device->setBaudRate(serialPortConfig->baudRate);         //　波特率
       serialPort_device->setDataBits(serialPortConfig->dataBits);         //  数据位
       serialPort_device->setParity(serialPortConfig->parity);             //  校验位
       serialPort_device->setStopBits(serialPortConfig->stopBits);         //  停止位
       serialPort_device->setFlowControl(QSerialPort::NoFlowControl);      //  流控制
       qDebug() << "串口打开成功:" << serialPort_device->portName();
   }
   else
    {
       qDebug() << "串口打开失败:" << serialPort_device->portName();
        return -1;
    }

    return 1;
}

int ImuDevice::initialModbusMaster(SerialPortConfig *serialPortConfigIn, int aliasIdIn)
{


    if(NULL==modbusMaster)
    {
        QString tmpName="/dev/";
        serialPortConfigIn->serialDeviceName=tmpName+serialPortConfigIn->serialDeviceName;

        QString tmpParity;
        if(QSerialPort::NoParity==serialPortConfigIn->parity)
        {
            tmpParity='N';
        }
        else if(QSerialPort::EvenParity==serialPortConfigIn->parity)
        {
            tmpParity='E';
        }
        else if(QSerialPort::OddParity==serialPortConfigIn->parity)
        {
            tmpParity='O';
        }

        modbusMaster =modbus_new_rtu(serialPortConfigIn->serialDeviceName.toStdString().data(),
                                     serialPortConfigIn->baudRate, (*tmpParity.toStdString().data()),
                                            serialPortConfigIn->dataBits, serialPortConfigIn->stopBits);
        modbus_set_slave(modbusMaster,aliasIdIn);
        if (modbus_connect(modbusMaster) == -1)
        {
            qDebug()<<"BatteryMonitor::connectPort connect failed";
            modbus_free(modbusMaster);
            modbusMaster=NULL;
            return -2;
        }
        else
        {
            modbus_set_response_timeout(modbusMaster, 0, 500000);
//            qDebug()<<"connect success";
            return 1;
        }
    }


    return 0;
}



void ImuDevice::receiveData()
{

    //数据预判断
//    int bufferSize=serialPort_device->bytesAvailable();
//    qDebug()<<"bytesAvailable"<<bufferSize;
    QByteArray receiveBatteryData;
//    int numRead = 0;
//    char buffer[500];
//    serialPort_device->waitForReadyRead(10);
//    numRead  = serialPort_device->read(buffer, 500);
////  if (numRead == 0 && !serial.waitForReadyRead())
//    receiveBatteryData.resize(numRead);
//    for(int i=0;i<numRead;i++)
//    {
//        receiveBatteryData[i]=buffer[i];
//    }

    switch (imuVendorId)
    {
    case E_IMU_VENDOR_ID_WIT:
    {
        receiveBatteryData = serialPort_device->readAll();

//            int numRead = 0;
//            uint8_t buffer[500];
//            numRead  = modbus_receive(modbusMaster, buffer);//会一直卡在这里

//            receiveBatteryData.resize(numRead);
//            for(int i=0;i<numRead;i++)
//            {
//                receiveBatteryData[i]=buffer[i];
//            }


//        qDebug()<<receiveBatteryData;
    }
        break;
//    case E_BATTERY_VENDOR_ID_shangHaiJinQu:
//    {
//        if(34>bufferSize)
//        {
//            return 1;
//        }
//        receiveBatteryData.fill(0);
//        receiveBatteryData = serialPort_device->readAll();
//        int tmpReturn;
//        tmpReturn=shangHaiJinQu_bufferCheck(receiveBatteryData);
//        if(1!=tmpReturn)
//        {
//            return -1;
//        }
//    }
//        break;


    default:
        break;
    }



    switch (imuVendorId)
    {
    case E_IMU_VENDOR_ID_WIT:
        if(1==receiveRawDataOnce_WIT(receiveBatteryData))
        {
            requestCount=0;
        }
        imuData.requestCount=requestCount;
        break;

    default:
        break;
    }



    return ;
}

int ImuDevice::receiveRawDataOnce_WIT(QByteArray &receiveBatteryData)
{

    int tmpKey=0;
    static int staticLength=0;
    static char staticBuffer[1000];//全部数据
    int tmpLength = receiveBatteryData.size();
    if(receiveBatteryData.size()>1000-staticLength)
    {
        qDebug()<<"receiveRawDataOnce_WIT 数据溢出";
        return -1;
    }
    for(int i=0;i<receiveBatteryData.size();i++)
    {
        staticBuffer[staticLength+i]=receiveBatteryData.at(i);
    }

    unsigned char tmpData[1000];//待解析数据
    if (tmpLength>0)
    {
            staticLength += tmpLength;
            while (staticLength >= 11)
            {
                memcpy(tmpData,staticBuffer,staticLength);
                if (!((tmpData[0] == 0x55) & ((tmpData[1] == 0x51) | (tmpData[1] == 0x52) | (tmpData[1] == 0x53))))
                {
                    for (int i = 1; i < staticLength; i++) staticBuffer[i - 1] = staticBuffer[i];
                    staticLength--;
                    continue;
                }
                decodeIMUData(tmpData,imuData);
                tmpKey=1;
                for (int i = 11; i < staticLength; i++) staticBuffer[i - 11] = staticBuffer[i];
                staticLength -= 11;
            }
    }
    return tmpKey;
}

